package demo;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.io.Serializable;
import java.util.Scanner;

import javax.vecmath.Point2d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;
import robot.*;
import program.*;
import pathAlgorithms.WeightedRoboGraph;
import simbad.sim.Box;
import simbad.sim.EnvironmentDescription;


public class EnvMovingObject extends EnvironmentDescription implements Serializable {
	private static final long serialVersionUID = -8760563281004784664L;
	public final static float BOX_SIZE = 0.9f;
	public static Vector3f boxVector = new Vector3f(BOX_SIZE,BOX_SIZE,BOX_SIZE);
	private final Point2d goalPos;
	private final Vector3d robotPos;
	
	public EnvMovingObject() {
		
		this.setUsePhysics(true);
 		this.showAxis(true);
		this.setUsePhysics(true);
		this.goalPos = new Point2d(0,-9);
		this.robotPos = new Vector3d(0,0,6);
		WeightedRoboGraph.GLOBAL_START_POS = new Point2d(robotPos.x, robotPos.z);
		MovingObjects mo = new MovingObjects();
		
		Robot r1 = new DStarDynRobot(robotPos, "DStarDyn", goalPos, mo);
		//Robot r1 = new DStarRobot(robotPos, "Dstar", new AStar_DStarPathTime(), goalPos, mo);
		Robot r2 = new DaggensRobot(new Vector3d (-2, 0 , 4), "Statist", mo,
				new Point2d[] {new Point2d(2, 4), new Point2d(-2, 4)});
		Robot r3 = new DaggensRobot(new Vector3d (-2, 0 , 0), "Statist", mo,
				new Point2d[] {new Point2d(2, 0), new Point2d(-2, 0)});
		Robot r4 = new DaggensRobot(new Vector3d (-2, 0 , -4), "Statist", mo,
				new Point2d[] {new Point2d(2, -4), new Point2d(-2, -4)});
 		mo.addRobot(r1);
 		add(r1);
 		
 		mo.addRobot(r2);
 		add(r2);
 		
 		mo.addRobot(r3);
 		add(r3);
 		
 		mo.addRobot(r4);
 		add(r4);
	}
	
	public void writeMapToFile ( String filePath, String map) throws IOException {
		File file = new File(filePath);
		FileWriter fw = new FileWriter(file);
		fw.write(map);
		fw.close();
	}
	
	public void writeRandomMapToFile (String filePath, double boxChans , int dimX, int dimY) throws IOException {
		
		int length = (dimX+1)*dimY - 1;
		char[] output = new char[length];
		
		int index = 0;
		
		for(int i = 0; i < dimY; i++) {
			if(i != 0)
				output[index++] = '\n';
			for(int j = 0; j < dimX; j++) {
				if(i == 0 || j == 0 || i == dimY-1 || j == dimX-1)
					output[index++] = '1';
				else if(i == 1 || j == 1 || i == dimY-2 || j == dimX-2)
					output[index++] = '0';
				else if(Math.random() <= boxChans)
					output[index++] = '1';
				else 
					output[index++] = '0';
			}
		}
		

		
		File file = new File(filePath);
		FileWriter fw = new FileWriter(file);
		fw.write(output);
		fw.close();
	}
	
	public void readMap (String filePath, int offsetx, int offsety) throws IOException {
		File file = new File(filePath);
		Scanner sc = new Scanner(file);
		
		int i = 0;
		
		while (sc.hasNextLine()) {
			
			String line = sc.nextLine();
			line.trim();
			char[] charLine = line.toCharArray();
			
			for(int j = 0; j < charLine.length; j++) {
				if(charLine[j] == '1')
					add(new Box(new Vector3d(j-offsetx, 0 ,i-offsety), boxVector, this));
			}
			i++;
		}
	}

	public void addBoxes(int mapNr) {
		
		switch (mapNr) {
			case 0 : 
				//Empty
			break;
				
			case 1 :
				//Simple map
				add(new Box(new Vector3d(-2,0,0), boxVector, this));
				add(new Box(new Vector3d(-3,0,-1), boxVector, this));
				add(new Box(new Vector3d(-3,0,-2), boxVector, this));
				add(new Box(new Vector3d(0,0,1), boxVector, this));
				add(new Box(new Vector3d(1,0,0), boxVector, this));
				add(new Box(new Vector3d(1,0,-1), boxVector, this));
				add(new Box(new Vector3d(-1,0,0), boxVector, this));
			break;
			
			case 2 : 
				for(int i = -10; i <= 10; i++) {
					add(new Box(new Vector3d(i,0,10), boxVector, this));
					add(new Box(new Vector3d(i,0,-10), boxVector, this));
					add(new Box(new Vector3d(10,0,i), boxVector, this));
					add(new Box(new Vector3d(-10,0,i), boxVector, this));
				}
				
				for(int i = 2; i < 10; i++) {
					add(new Box(new Vector3d(-i,0,0), boxVector, this));
				}
					
				for(int i = 1; i < 8; i++) {
					add(new Box(new Vector3d(1,0,i), boxVector, this));
					add(new Box(new Vector3d(-1,0,i), boxVector, this));
					add(new Box(new Vector3d(1,0,-i), boxVector, this));
					add(new Box(new Vector3d(-1,0,-i), boxVector, this));
				}
				add(new Box(new Vector3d(0,0,-8), boxVector, this));
				
				add(new Box(new Vector3d(-1,0,0), boxVector, this));
				add(new Box(new Vector3d(0,0,2), boxVector, this));
				add(new Box(new Vector3d(1,0,0), boxVector, this));
				break;
			
			case 3 : 
				for(int i = -8; i <= 8; i++) {
					for(int j = -8; j <= 8; j++) {
						if((robotPos.x != i || robotPos.z != j) && !(i == goalPos.x && j == goalPos.y) && Math.random() < 0.33)
							add(new Box(new Vector3d(i,0,j), boxVector, this));
					}
					
				}
				for(int i = -10; i <= 10; i++)
				{
					add(new Box(new Vector3d(i,0,10), boxVector, this));
					add(new Box(new Vector3d(i,0,-10), boxVector, this));
					add(new Box(new Vector3d(10,0,i), boxVector, this));
					add(new Box(new Vector3d(-10,0,i), boxVector, this));
				}
		}	
	}
}